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Abstract 

The Payload Changeout Room Inspection and Processing System (PIPS) is a highly redu “ 

p° tentiai *• b « * » p* ,h p'™”" 8 tor * te PIPS “ 

the Shuttle Payload Bay environment. 
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I INTRODUCTION 


1 Introduction 

The range of motion achievable by a robot manipulator’s end effector is a function of the number 
and type of joints or degrees of freedom it possesses. Any degrees of freedom m excess of the 
minimum number required to reach an arbitrary end effector positron and orientation within the 
reachable workspace are considered ’’redundant”. Commercial manipulators typically possess six or 
fewer DOF for primarily "anthropomorphic” tasks such as industrial assembly and are therefore not 

redundant. 

There are some tasks for which such standard manipulators are not well suited, such as^those 
requiring an extended reach in a confined workspace. For that reason, so-called serpentine ma- 
nipulators have attracted interest. Their designation and appearance suggest the longreachand 
dexterity associated with snakes or tentacles. They achieve this snake-like ability by possessing a 
high degree of redundancy. This redundancy allows them, theoretically, to wriggle an end effect 
into a confined or difficult to reach point while allowing the robot arm to be configured in such a 
way as to not contact the surrounding environment. 

The Payload Processing and Inspection System seeks to exploit the dexterity of the serpentine 
truss to service space shuttle orbiter payloads in the Payload Changeout Room (PCR )• Because of 
the dimensions of the PCR and the sensitivity of shuttle payloads, there are specific tasks which are 
difficult , costly or hazardous to perform by humans due to lack of access. These tasks include. 

« photographic inspections. 

• visual inspections 

• spot cleaning 

• cover installation and removal 

• line replaceable unit (LRU) installation and removal 

• connector installation and removal. 

References [2] and [l] discuss the requirements for inspection and processing of space related 
oavloads and the feasibility for employing a manipulator to perform such tasks. 

P t.ve r.pproacl, eS for thieving collision avoidance with redundant ™mpula«ors 
suggested Mad ejewski and Klein [3], Nakamura [4], and Wegenf, et td [5] make use of the Moore- 
Pemose pseudo-inverse [6] to generate the joint rates to move the end effector and null motion t 
avoid obstacles The pseudo-inverse solution is hampered by the existence of singularities for which 
troseud^vers s undefined. Under these circumstances, no motion in the specified direction is 
plS S and Sicilian. (7) make us. of a Lyapunov stability function to track a pre c„b«l 
trajectory and augment the configuration space to a accommodate obstacle avoidance constr . 
An alternative approach is used by Pasch [2] and Asano [8] They 

effector path and cause each joint to adhere to that path in a “follow the leader mode All of these 
methods require that at least the end effector’s trajectory and velocity be prescnbed^This presumes 
that a suitable velocity function for the end effector is readily determined. On y egen [ ], 
makes use of sensors to detect obstacle proximity, allows for the end effector to deviate from the 

prescribed path as an emergency measure. 

There are several limitations inherent in these approaches. The pseudo inverse kinematic : solu- 
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presumes that a suitable trajectory is easily determined. Such a trajectory must not only itself be 
obstacle and singularity free, but must allow for the permissible motion of the entrained links. Null 
motion may not be sufficient to cause the entrained links to avoid obstacles because such motion is 
constrained by the end effector trajectory requirements. Furthermore, as discussed by Doty, et al, 
[10] the pseudoinverse solution to robot manipulator kinematics can lead to inconsistent results (i.c. 
results that are not invariant with respect to changes in the reference frame and/or changes in the 
dimensional units used to express the problem). 

In Ref. [11] the principal investigator presented an alternative method for determining an accept- 
able robot trajectory which allows the end effector’s path, as well as the entrained link’s to be free 
to move around obstacles. The control algorithm uses a Lyapunov stability approach to generate a 
family of joint rates which will move the end effector toward a desired target. The relative motion 
of the joints can be weighted to meet operational requirements such as rate or deflection limits. 
Because the end effector path is not specified, there are no requirements for inverse solutions, and 
singular joint configurations are only encountered at the reachable workspace boundaries. 

Obstacles are avoided by determining the distance from each link to the surface of each obstacle 
in the workspace. An obstacle gradient vector, indicates the direction, in the joints space toward 
the obstacle array. By selecting only joint motion which is orthogonal to this direction, collisions 
with obstacles are avoided. 

In the current work , the collision avoidance algorithm is applied to a notional PIPS based on the 
Foster-Miller serpentine truss [2] with sixteen degrees of freedom. Both the end effector’s desired 
final position and orientation may be specified. The algorithm is coded in the C programming 
language and graphically displayed using the IGRIP software. 


2 Manipulator Kinematics 

Typically, robot motion is sufficiently slow so that it is adequately controlled by commanding joint 
velocities in response to the robot kinematics. Serpentine motion and the requirements for collision 
avoidance are especially complex. It is sufficient to describe the motion in terms of the end effector 
position and velocity. 


The end effector position is a function of the vector of generalized joint displacements q. 


L = r(q(t),t) € 


(i) 


Figure 1 illustrates that the location of a point in space given by the 3 x lvector r can be expressed 
in terms of an inertial frame by its position in an intermediate frame, the location of the origin 
of the intermediate frame, r^and the orientation of that frame with respect to the inertial frame, 
given by the 3x3 direction cosine matrix Rq . 

L = RoLb + La ( 2 ) 

It is appealing to express the transformation in the form 

L — TqLb ( 3 ) 
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yA 



Figure 1: General Transform of a Vector 


This is accomplished by defining the 4x4 transformation matrix relationship 


r 1 


- 

1 


“■ 

r 


R-o 

1 La 
1 — 


Lb 

. 1 . 


. 0 0 0 

1 1 - 


. 1 . 


The well known Denavitt-Hartenberg convention [13], is a convenient convention for describing 
the transformation between link coordinate frames and is shown in Fig. 2. The length a { is the 


Joint j 


+ 



Figure 2: Denavitt-Hartenberg Coordinate Transform Convention 


length of the common normal between the frames. For a revolute joint this is link length . T 
length di is the distance between the origin 0,_,and the point Hi. In a prismatic joint this is 
the variable component.The angle a, is the rotation of the joint axis i and the *< axis, about the 
common normal; the “twist” of the link. The angle 6 t is the rotation angle between the x,_i axis 
and the common normal ff.O; measured about the z;-i axis in the right an sense, n a re vo u 
joint, this is the variable parameter. In the D-H convention, the 4x4 transformation between link 
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frames is given by 


_ COS0j 

sin 

0 

. 0 


— sin Qi cos a,* 
cos Si cos a,- 
sin a,- 
0 


sin Si sin a, 
— cos0; sin a,- 
cos a* 

0 


a* cos0, “ 
a,- sin Si 
di 

1 . 


( 5 ) 


For a manipulator consisting of n links, the position and orientation of the end effector (frame 
n) with respect to an inertial frame (frame 0), is expressed in terms of the link transformations 

n 

t 0 u = n*t. 

»=i 


_ 


1 

R l* rg et | 

Liar get 


. 0 

0 0 1 

1 , 


n l 

Enj —n 3 En. 



0 

0 0 1 



The vectors r ni , r na , and r„ 3 are unit column vectors of the direction cosine matrix which relates 
the end effector’s orientation to the inertial frame. The vector r n4 gives the end effectors location 
with respect to the origin of the base frame. 


The velocity of the end effector is given by 


dr_ dr_ dq ^ m 

dt dq dt - 


( 7 ) 


where J is the Jacobian matrix. For an end effector trajectory specified by r the required joint rates 
are given by 

l = J'i (8) 

where 

J‘ = J T (JJ T )~ l (9) 

is the pseudo-inverse for n > 3. Equation (8) gives the minimum norm joint rates which satisfy the 
end effector trajectory r. When \JJ T \ = 0 the pseudo inverse is undefined and infinite joint rates 
are required to satisfy the specified end effector velocity. Obviously, even when the manipulator is 
in a singular configuration, it is still possible to move the end effector in directions other than the 
singular direction. 


There are several limitations to the pseudo inverse velocity kinematics solution of robot motion. 
As with all pseudo inverse kinematic solutions, the end effector’s trajectory must be specified and 
takes priority over obstacle avoidance. Choosing an acceptable end effector path can be a difficult 
task in a complex workspace and it sometimes occurs that the specified path precludes obstacle 
avoidance. To further complicate matters, null motion for obstacle avoidance may be incompatible 
with the task of singularity avoidance. Finally, Doty, et al [10] notes that noninvariant results may 
be obtained from the pseudo-inverse solution. 
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3 LYAPUNOV STABILITY APPROACH TO MANIPULATOR CONTROL 

3 Lyapunov Stability Approach to Manipulator Control 

As an alternative to the operator prescribing the end effector path, the end effector may be driven 
to its target by use of a Lyapunov stability function. The desired end effector position may be 
represented by a target transformation 

retarget 


1 

R ta,gel | 

—target 

. 0 0 0 | 

1 . 

S 

8 

s 

£ 

M 


[ 0 0 0 1 . 



( 10 ) 


where r T , i = 1,2,3, are the unit column vectors of R l £ r9et and r T - L target . The difference 
between the manipulator’s actual configuration and the desired configuration is given by the array 

of vectors u = r Tl -r_„, <»> 

The scalar Lyapunov function is chosen 

v = t, k T a < 12 > 

i=l Z 

V may be viewed as the “energy” of the system and is always positive. To drive V to zero, and 
hence the end effector to the target position and orientation , it is sufficient that V < 0 for every 
subinterval of time on to < t < t / . 


Taking the time derivative of V gives 

V = 


Eifi- 

»-i 

-i> r ( j *t) 


i=i 


where 


Ji = 


dr ni 

dq 


* = 1, ... 4 


It is obvious that V < 0 is guaranteed by choosing the joint rate vector 

t 

where M is a n x n positive definite scaling matrix, Eq. (13) becomes 

■ _ v— ' S.T JjMJj _ £j 

T - " h h^ii 


(13) 

(14) 

(15) 

(16) 
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which is always negative. Substituting Eq. (15) into Eq. (7) gives 

r = (17) 

t=l 

No matrix inversion is required, and therefore the control is not sensitive to singularities. In contrast 
to Eq. (8) which gives joint rate to satisfy a desired trajectory, Eq. (17) moves the end effector in 
response to a family of joint rates which depend on the relative priority of joint motion caused by 
the matrix M. In addition, this matrix enforces appropriate unit transformations. 


4 Joint Motion Weighting 

Generally, the boundary conditions and obstacle avoidance requirements can be satisfied by an 
infinite number of joint trajectories by modification of the M matrix. The composition of the M 
matrix is determined by the various requirements on the hardware or end effector task. 

In addition to avoiding obstacles, manipulator arms are frequently limited by the manipulator 
architecture in the magnitude of the joint deflections and joint rates which can be achieved. The M 
matrix may be selected to enforce joint rate and joint displacement limits. 


It is useful to think of the M matrix as the non-linear stiffness matrix. The deflection of the ith 
joint is bounded by q ijnin < < g lm „. Defining 


Ai — 9 l m«* 

= Qi max "h 

, _ 2*i-r £ 

fi ” A t - 


rji = sign (e;) 



The elements of M are given by 


TTlij = 


ki(l — f]ifi) 
0 


* = j 
i 


(18) 


Equation (18) causes the ith joint rate toward the joint limit to approach zero near the limit and 
the rate to be near the maximum if away from the limit. 


5 Obstacle Avoidance 

With the end effector motion no longer prescribed, much greater latitude is allowed in obstacle avoid- 
ance. Joint motion which moves the manipulator away from obstructions is no longer subordinated 
the end effector path. 

Obstacle avoidance requires that the distance to obstacles v wis-a-i/is the manipulator links be 
known. In a realistic environment, devices in the workspace may be numerous and complexly shaped. 
CAD models of high complexity, such as exist in the Payload Changeout Room may be imported 
to IGRIP. The MINJDISTANCE function in the GSL language returns the minimum designated 
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links and devices in the CAD environment. IGRIP can be set to disregard any devices outside of a 
selected radius. 

The cost function C {j is the minimum distance between the ith link and the jth obstacle (Fig. 
3). Contact of the ith link with the jth obstacle is indicated by G,-; - U. • 



The potential function 


nl no - 

»=i j = i 11 


(19) 


dP 


n = 


dq 


The time rate of change of P can thus be expressed 

dP 


T ■ 

dt = £ q - 


( 20 ) 


( 21 ) 


Assuming that a trajectory 

of L which is orthogonal to £ is found via 

the Gram-Schmidt orthogonalization method. 


£ = 2r _ (iri) £ 

where £ is a unit vector in the direction t Equation (22) may be written 

a = [s» - (a£ t )] 2r 


( 22 ) 


where 


q = Me 

M = (E n - /t£ T ) A/ 


(23) 

(24) 
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M is the obstacle avoidance metric. This matrix is positive semi-definite. The fact that this matrix 
possess a zero eigenvalue becomes evident when p is parallel to £ r . In this circumstance, it is 
impossible for the end efFector to move closer to the target. The most obvious case occurs when 
the target is unreachable or the manipulator has entered a dead-end path. The operator may take 
some steps to avoid the manipulator from entering a dead-end by designating intermediate targets, 
or waypoints. 


As an alternative to measuring distances from the links to devices in the workspace, obstacles 
may be modelled as primitive solids. In this approach, the centroid of the jth object is located at 
Oj = [xj yj Zj ] T and has the dimensions 2a j, 2b j, 2cj, along its principal axes. The orientation of 
the solid with respect to the inertial frame is given by a direction cosine matrix. Obstacle avoidance 
points pi, i = 1,... ,7i p are designated along the manipulator arm. In the simulation model, these 
points are the joints and the link midpoints.The location of the jth obstacle vis-a-vis the ith obstacle 
avoidance point is approximated by the super-ellipsoid function 




(25) 


The the desired shape of the jth obstacle is approximated by selecting appropriate values for fcjf, 
and kj greater than or equal to one. For fcjf = kj = 1 the surface is an octahedron. Setting 

&J,= kV = kj = 8 approximates a rectangular parallelopiped. Contact with the surface of the 
jth obstacle by the ith collision avoidance point is approximated by Cj(pi) — 1. The workspace 
potential function is defined by 

p =EEi c 'i(p')-r 1 (26) 

i * 

where np is the number of collision avoidance points. The gradient vector £ is generated by a finite 
difference method as described above. 


6 Graphical Representation with the IGRIP software 

The Interactive Graphical Robot Instructional Program is a product of Deneb Robotics Inc. [12] 
It is a computer graphics based package for workcell layout, simulation and offline programming 
which permits the graphical simulation of virtually any robotic device. Devices used in the workcells 
may be added by modelling them with any of several CAD systems. A device has both geometric 
and non-geomentric information stored with it, Non-geometric information including kinematics, 
dynamics, velocities, etc., can be entered through interactive menus. 

IGRIP allows robot programming via the Graphical Simulation Language (GSL), which in turn, 
can communicate with programs written in C programming language. This capability will eventually 
be exploited to imbed the robot control algorithm into the IGRIP simulation. 

7 The Payload Inspection and Processing System 

The Payload Inspection and Processing System (PIPS) is conceived as a highly redundant manipula- 
tor with a serpentine truss configuration. It is based on the Foster-Miller Serpentine Truss currently 
under development at the Kennedy Space Center. The truss shown in Fig. 4 can accomodate up to 
twelve degrees of freedom. 

For the purposes of examing the efficacy of control algorithms, a notional PIPS, shown in Fig 5 
has been designed. The illustration was generated in IGRIP. The Foster Miller Truss, with twelve 
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Figure 4: Foster Miller Serpentine Truss 



Figure 5; Notional PIPS 
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degrees of freedom, is mounted to a pedastal with two revolute joints and three telescoping prismatic 
joints. At the end of the truss, a revolute wrist is mounted, giving the complete system eighteen 
joints and sixteen independent degrees of freedom. The table of the Denavitt-Hartenberg parameters 
for the nominal “home” position, is shown in Table 1. 


Table 1: Dencvitt-Hartenberg Parameters for Notional PIPS 


i 

a.'(deg) 

0»(deg) 

Of (in) 

di (in) 

joint type 

1 


90.0 

0.0 

22.0 

revolute 

2 

90.0 

0.0 



revolute 

3 

0.0 

0.0 

-2.593 

20.0 

prismatic 

4 

0.0 

-45.0 


HflBEHH 

prismatic 

5 

HBrifiEEflN 


0.0 

MM 

prismatic 

6 

HHEQSBHI 


20.0 

0.0 

revolute 

7 

-90.0 


1.25 

0.0 

revolute 

8 



20.0 

0.0 

revolute 

9 


0.0 


0.0 

revolute 

10 

90.0 

o 

o 

16.004 

0.0 

revolute 

11 

-90.0 


1.188 

0.0 

revolute 

12 

90.0 

WBBEEMM 

16.004 

0.0 

re volute 

13 

-90.0 

HHHEMI 


0.0 

revolute 

14 




0.0 

revolute 

15 

-90.0 

wmmum 

.813 

0.0 

revolute 

16 

90.0 

0.0 


0.0 

re volute 

17 

-90.0 


.833 

-.833 

revolute 

18 

0.0 


0.0 

1.793 

revolute 


8 Algorithm Implementation 

The algorithm described above is executed in the C language program, Collision Avoidance Path 
Planner (CAPP.c). The program flow is shown in Fig.6(a). In order to make use of utilities imbedded 
in IGRIP, CAPP will itself become a library utility which can be accessed by a program written in 
a GSL program which directly controls the graphical simulation and shown in Fig. 6(b). 

The CAPP program has demonstrated the ability to generate obstacle free trajectories for the 
PIPS model. In Fig7 the PIPS is shown manevering in a simple representation of the PCR/Shuttle 
Payload Bay environment. In the simulation, it is desired to view a point, to the aft of the large 
cylindrical payload from a distance of six inches. 

9 Conclusions and Recommendation 

An algorithm has been presented which will move the end effector of a redundant manipulator toward 
a target state while avoiding collisions of the arm with obstacles in the workspace. Allowing the end 
effector path to be free avoids the problem of singularities found in the pseudo-inverse solution of 
the robot kinematics. In addtion, it simplifies the operator’s workload and allows greather latitude 
for obstacle avoidance. The algorithm is straightforward and requires only modest computing power. 
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Although it is applied here to a highly redundant manipulator, redundancy is not explicitly required 
for its implementation- 

Serpentine manipulators such as the PIPs are envisioned for employment in complex and costly 
environments This method provides a tool for path planning by which specific maneuvers may 
sZlZ ^lt risk. A nominal joint history may be generated which is subsequently used as an 
open loop trajectory to be tracked by a robot with distributed control. 

Offline processing of the robot trajectory, while sufficient to demonstrate the efficacy of the CAPP 
algorithm is cumbersome and has severe shortcomings. Equation (25) has only limited utility to 
model a^'om ptex* environ ment , such as exists in the shuttle payload bay. Processing time increases 
dramatically as the number and complexity of obstacles increases beyond a few simple shapes. 

For that reason, it is recommended that future research be directed at various 
•iri- „ rQT and C laneuaees in IGRIP. This will allow the algorithm to interrogate IGRiF 
to given by the MIN -DISTANCE utility. This should allow ve,y complex and 

realisitic CAD models to be exploited and greatly reduced execution time. 

There are several unresolved problems with automated path planning. In its current incarnation, 

the operator to have specific rules by which to chose an opumal confiprat,on : Jhe we^gh mg 
the joint motion is also somewhat arbitrary, currently only intocmg joint rate limits. The g 

between revolute and prismatic joints requires a more rigorous basis. 

Currently the operator may designate way-points which assist the algorithm in ^ding a collision 
free^patl^Howeverf heuristics should be developed which help the manipulator avoid dead-ends and 
to choose between multiple paths around an obstacle. 
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A Appendix: Collision Avoidance Path Planner Source Code Listing 


/A******************************************************************* 

* COLLISION AVOIDANCE PATH PLANNER 

* Dr. Robert M. Byers, Unversity of Central Florida 

* 8/4/94 

* Robot end effector directed to a point in space with 

* a desired orientation , 

* Obstacles are modelled by hyperellipsoids 

* and may be oriented via 1-2-3 euler angles 

* Robot parameters contained in 'input.dat' 

* joint angles written to ' joints. dat' 
*********************************************************************/ 
tinclude <stdio,h> 

#include <math.h> 

/************************************* 


function prototypes 


* * * j 


void mat r ixjnult (float* *matrixl, f loat**matrix2) ; 

void end_ef f ector {int n, float*q p. float target_p(3] [4] , f loat error_p[3] [4] , float err_mag [ 4 ] ) ; 
void integrate (int n, float *varl, float *var2 ( float err, float err_dot, float step_s) ; 
void joints_print ( int n, float *var, FILE *file) ; 

void **obstacle_trans format ion (int n, float **obst, float vector_n [ 3 J ) ; 

float **IdentityMatrix(int n) ; 

float ***JacobianMatrix(int n_dof, int n_obs, float * g o . float **ob) ; 
float **transformation_jnatrix(int n, float *var} ; 

float * joint_rates ( int n, float *metric_p, float error_p [3] [4] , float target__p [3] [4] , 

float***jb, float step_s) ; 

float *mem_alloc_l (int n) ; 

float **menu.alloc_2 (int nrows, int ncols); 


int *flag; 

float *alpha, *theta, *a, *d; 

/**************************** * MAIN* * **********************************/ 


main ( ) 

{ 

/********************** 

* local variables * 

**********************/ 

int i, j,k, num_dof, num_obstacles, num_waypoints, waypoint_counter ; 

float ***jacobian; 

float *rate,*q; 

float *metric; 

float **obstacle; 

float **waypoint; 

float target [3] [4] , error [3] [4]; 

float ratejnag, tolerance, move_dist; 

float error_mag [4] , step_size, error_mag_old, error_prod. error_prod_old, error_prod_dist ; 
float error_step=-l .0; 

FILE *data; 

FILE * joints; 

joints=fopen( “joints .dat • , *w" ) ; 

if ( (data=fopen( "input .dat" , "r") ) ==NULL) 

{ 

printf ( "input file could not be opened\n“); 
exi t ( - 1 ) ; 

} 


j * ************************* 

* input data 
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fscanf (data, "%d’ f &num_dof) ; 

alpha=mem_al loc_l (num_dof ) ; 
theta=mem_ alloc_l (nuro_dof ) ; 
a=mem_aIloc_l {nunudof } ; 
d=raem_alloc_l (num_dof ) ; 
metric=mem_alloc_l (num_dof ) ; 
q=mem_alloc_l (num_dof ) ; 

if ( (f lag= (int* ) malloc { sizeof (int ) *nuro_dof ) ) == (int *) NULL) 

{ 

fprintf (stderr, ’Error mallocing flag\n’); 
exit ( -1) ; 

} 


for (i=0; i<nufl\_dof ; i++) 

fscanf (data, •%£ %f %f « %£ %d\n* , &alpha[i], 6theta(i], ta(i] 

&metric[ij ,&flag(i] ) ; 

if (flag [i] ==1) 

q[i] =theta[i] ; 
else 

q [ i ] =d [ i ] ; 

) 

/********************************** 

* read in target information 

* and way points 
**********************************/ 
waypoint_counter=0; 

fscanf (data, -%f %f %d‘. 6step_size, ^tolerance, t nunuwaypotnts) ; 
waypoint =mem_al 1 oc_2 ( 3 , nunu.waypoint s ) ; 


for <i=0 ; i<num_waypoints; i++) 

for ( j=0; j<3? j++) 

fscanf {data, ’%f ", &waypoint[j](i]); 

for (i=0; i<3; i++) 

target [i] [3] =waypoint [i) [0] ; 

for ( i=0; i<3 ; i++) 

for ( j=0; j<3; j*-*-) 

fscanf (data, ’%f ’,&target[i] [ j] ) ; 

^********************* # *********** 

* read obstacle array 
*********************************/ 

fscanf (data, ’%d*. &nunt_obstacles) ; 

obst ac 1 e=mem_al 1 oc__2 { 12 , nunv_obstacles) ; 


for (i=0;i<nuin_obstacles; i + +) 
for { j=0; j<12; j++) 

fscanf (data, ’%f", ^obstacle b J U 1 ) J 


f close (data) ; 


&d ( i 1 , 
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joints_print (num_dof , q, joints) ; 

/ * ******44**************************************** 

*. for loop until all waypoints passed 

***4*4* ******44* *************** ********4***4*****4/ 


while (waypoint_counter<num_waypoints) 

{ 

Z**************************************** 

* determine end effector position 

*****4****44**********************4*4***/ 

end_ef fector ( num_dof, q, target , error, error_mag) ; 

for ( j=0; j<4; j++) 

printf(*%f " , error_mag [ j ] ) ; 
print f { "\n“ ) ; 

error_mag_old=error_mag [ 3 ) ; 
move_dist=error_mag__old? 

error prod=sqrt { error_mag [0 ] *error_jnag[0 ) +error_mag [1] *error_mag [ 1 ] +error_jnag [2 ] *error_raag [2 ] ) ; 
error_prod_old=error_prod ; 

/ * *****44*****4*********************************** 

* while loop until error within tolerance 

4*4*44*44444444444444*444*44*444444444**44**4*4444/ 

while (error_mag [3] >tolerance! I error_prod> . 4) 

{ 

z********************************** 

* form the jacobian matrix 

*44****4*44**4*4**4*444*444**4444* / 

jacobian=JacobianMatrix (num_dof , num_obstacles, q, obstacle); 

/******************4****444****4********************** 

* determine joint rate vector toward target 

4*44*4*4******4*44444*4444***44**44*4*****************'/ 

rate=joint_rates (num_dof , metric, error, target , jacobian, step_size) ; 

/* 4444444*4**44****** *4 *********************** ******** 

* integrate joint rates to update joint parameters 

*******44*********************************************/ 

integrate (num_dof, rate, q, error_mag[3] , error_step, step_size) ; 

/***#**4***4**44*4***44444*44444**4*****4***444*44**** 

* recompute position vector and error vector 

4**44**444*4*4444*4*444*444**444***44*44*4444*4**4*4*4/ 

end_ef fector { nunudof, q, target , error, error_mag) ; 
for ( j=0; j<4 ; j++) 

printf { “%f " , error_jnag [ j ] ) ; 

print fj[ “ \n“ ) ; 

error_step=error _mag [ 3 } -error__mag_old; 
error_mag_old=error_mag[3] ; 

error_prod=sqrt ( er ror_mag [ 0 ] * error _jnag[0] +error_mag [1] *error_mag[l]+error_mag[2] *error_mag [2] ) ; 
error_prod_old=error_prod; 

z*************************************** 

* print joint angles to “joints.dat" 

4**4************************************/ 

if ( f abs (move_dist-error_mag [ 3 J ) >1 . 0 II f abs (error_prod_dist-error_prod) > . 1) 

{ 

move_dist=error_mag[3] ; 
er ror_prod_di st =error_pr od ; 
joints _print (num_dof , q, joints) ; 
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) 


} /* end error tolerance while*/ 

/**♦***♦*♦** ******************************* 

i f ( waypoint_counter==num_waypo i nt s - 1 ) 

printf (“Target point reached\n"); 
printf ( *\n Waypoint %d reached\n 


****************** 




waypoint_counter) ; 


waypoint_counter+=l ; 


for (i=0? i<3; i++) 

target [i] [3] =waypoint [i] [waypomt.counter J ; 
} /*close waypoint counter while loop */ 

/**★***************************************** 

* print final joint angles to "joints.dat" 

a****************************************"**/ 

jnints p rint (nuiiL-dof , q, joints) ; 
f close (joints) ; 


free (alpha) ; 
free (theta) ; 
free(a) ; 
free (d) ; 
free (metric) ; 
free (flag) ; 
f ree(q) ; 
free (rate ) ; 


for (i=0 ; i<12; i++) 

free (obstacle [i 3 ) ; 
free (obstacle) ; 
for (i=0; i<3; i++) 

f ree (waypoint [i] ) ? 
free (waypoint) ; 


for (i=0 ; i<3; i++) 

( 

for ( j=0; j<num_dof ; j++) 

free ( j acobian [ i ] ( j ] ) ; 
free ( j acobian ( i ] ) ; 

) 

free (j acobian) ; 


/A******************************** 

* end of main program 
****************** ***************/ 
/**♦************ * ‘TRANSFORMATION MATRIX**** 
float **trans format ion_jnatrix(int n, float 
{ 

static float "transform: 


******* 

*var) 


*****/ 


int i ; 


if ( ! (transform) ) { 
transforms (float ** 
for(i=0;i<4;i++) 


malloc ( sizeof ( float* ) *4 ) ; 
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} 


if ( (transform[i3=( float * )malloc (sizeof (float) *4) ) == (float *) NULL) 
fprintf (stderr, "Error mallocing transf orm\n" ) ; 


/♦rotation matrix*/ 
if ( flag [n] ==1) 

{ 

transf orm[0] [0] =cos (var [n] ) ; 

transf orm[0] 11] =-cos (alpha [n] ) * sin (var [n] ) ; 

transform [0] [2] = sin (alpha [n] ) * sin (var [n] ) ; 

transform [0] [3] =a [n] *cos (var [n]); 

transform [1] [0] = sin (var [n] ) ; 

transf orm[l] [1] =cos (alpha [n] ) *cos ( var [n] ) ; 

transform[l] [2 J =-sin (alpha [n] ) *cos (var[n] ) ; 

transform [1] [ 3] =a [n] * sin (var [n] ) ; 

transform [2] [0]=0; 

transform [2] [1] =sin (alpha [n] ) ? 

transform[2] [2] =cos (alpha [n] ) ; 

transform [2] [3 J =d[n) ; 

transform(3] ( 0 ) = 0 ; 

transform [3] (1]=0; 

transform[3] [2]=0; 

transform(3] [3]=1; 

} 

else 

{ 

transform [0] [0] =cos (theta [n] ) ; 

transform [0] [1] =-cos (alpha [n] ) * sin (theta [n] ) ; 

transf onu[0] [2] =sin (alpha [n] ) * sin (theta [n] ) ; 

transform [03 [3]=a[n] * cos ( theta [n] ) ; 

transformfl] [0] =sin (theta [n] ) ; 

transform[l] [l]=cos (alpha [n] ) *cos (theta [n] ) ; 

transform[lJ [2]=-sin(alpha[n] ) *cos (theta [n ] ) ; 

transform [1 ] [3]=a[n] *sin( theta [n3 ) ; 

transfom[2] [03=0; 

transform[2] [1] =sin(alpha [n3 ) ; 

transf orm[2] [2] =cos (alpha [n) ) ; 

transf orm [2] [3] =var[n] ; 

transform[33 [0]=0; 

transform [33 [ 1 ] = 0 ; 

transform[3J [ 2 ] =0 ; 

transform [3] [33=1,* 

} 

return (transform) ; 

} 

/★★★★ft********** ★MATRIX MULTIPLICATION* ********★******★**★**/ 
void matrix^ mult ( float * *matrixl , float* *matrix2) 

{ 

float **matrix3=mem_alloc_2 (4 , 4) ; 
int i,j,k; 

for (i=0; i<4 ; i++) 

for < j=0; j<4 ; j++) { 

matrix3 (i ] [ j 3 =0; 
for (k=0;k<4;k++) 

matrix! [ i ] [j ) +=matrixl [ i ] [k] *matrix2 ( k ] [ jl ; ) 

for ( i=0 ; i<4 ; i*+ ) 

{ 
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free (matrixl [i] ) ; 
matrixl [i] =matrix3 ( i ] ; 

} 

} 

/**************♦*** * IDENTITY MATRIX* ******••**********/ 
float * *IdentityMatrix ( int n) 

{ 

float **matrix=mem_alloc__2 (n,n) ; 

int i , j ; 


f or ( i=0; i<n; i*+) 

for { j = 0; j<n; j + + ) 

if (i==j) 

matrix[i] { j] =1; 

else 

matrix!!} [ j ] =0 ? 

return (matrix) ; 


> 

! * * * * * 

float 

{ 

float 
float 
float 
float 
float 
float 
float 
float 
float 
float 
float 
float 
float 
int i 


*************** JACOBIAN MATRIX* ************************** 
***JacobianMatrix(int n__dof, int n_obs, float *q_p# float 

*** jacobian; 

**• temp; 

**result__plus, **result_minus; 

*q plus=mem_alloc_l (n_dof ) ; 

*q_jninus=mem_alloc_l (n_dof ) ; 

•gradient =mem_alloc l(n^dof) ; 

cost_piinus; 

cost_plus; 

**mu - matrix=mem_alloc_2 (n_dof , n^dof ) ; 
potent ial_plus , potent ial_jninus; 
end_point_plus [ 3] , end_point_minus [3 ] ; 
mid_point_plus [ 3 ] , mid_point__minus [33; 
gradient_mag=0 . 0; 

, j, k,kk; 


/ 

**ob) 


jacobian= (float*** Jmalloc (sizeof ( float* 4 ) *3); 
for (i=0? i<3 ; i++) 

jacobian [i] = { float** ) malloc (sizeof (float* ) *n_dof ) ; 
for ( j =0 ; j <n y _do£ ; j ++) 

jacobianti] [ j ]=<float*)malloc(sizeof (float) M) ; 

temp=( float***) malloc (sizeof (float**) *3) ; 
for (i=0; i<3; i++) 

temp [i] = (float* * ) malloc { sizeof (float*) *n_dof) ; 
for ( j=0; j<n_dof ; j++) 

temp (il [ j}=(float* Jmalloc (sizeof (float) *4) ; 

} 


/*****************♦•*•********************** 

* virtual joint displacement loop 

A********************************* 4 ********/ 
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for ( j = 0; j<rL_dof ; j + + ) / * outer loop start*/ 

{ 

for (i=0; i<n_dof ; i++) 
if <i==j> 

{ 

q plus Til - q p m -f . 005; 
q_jninus [i ] = q p [ i 1 - . 005; 

} 

else 

( 

q_plus[i]=q_p[i] ? 
qjninusCi] =q_p[i] ; 

}; 


result_plus=IdentityMatrix(4 ) ; 
result_minus=IdentityMatrix (4); 
potential_plus=0 . 0 ; 
potential_minus=0 . 0 ; 
for <i=0; i<3; i++) 

{ 

end_point_plus [i] =0 . 0; 
end_point_minus [i] =0. 0; 

) 

/♦find change in r for a plus/minus permutation of q*/ 

for ( i=0 ; i<n_dof ; i + + ) 

{ 

/****************************************************** 

* cost function for joint locations 

******************* ****** ************* * ****************/ 

matrix_mult (result_plus, transformation_jnatrix(i, g p lus) ) ; 
matrix_mult ( result_minus, transformation_matrix(i, q_minus) ) ; 

for (kk=0 ; kk<3 ; kk++) 

{ 

mid_point_plus [kk] = (result_plus [kk] [3] +end_point_plus [kk] ) /2 . 0; 
mid_point_minus [kk] = (result_minus [kk] [3] +endLpoint_minus [kk] ) /2 . 0; 
end_point_plus [kk] =result_plus [kk] [3 ] ; 
end_point_minus [kk] =result_minus [kk] [3] ; 

) 

/**************************************************** 

* link endpoint collsion avoidance cost function 
*****************************************************/ 

f or (k=0;k<n_obs; k+ + ) 

{ 

obstacle_transformation(k, ob, end_point_plus) ; 
obstacle_transformation(k, ob, endL_point_minus) ; 
obstacle_transf ormation(k, ob,raid_point_plus) ; 
obstacle_transf ormation{k, ob, micLpoint_minus) ; 

cost_plus= -1.0; 
cost_minus= -1.0; 
f or (kk=0 ; kk<3 ; kk++ ) 

( 

cost _plus+=pow( (end_point_plus [kk] -ob[kk] [k] ) / (ob[kk+3] [k] +6.0), ob[kk+6] [k] ) ; 

cost - jninus+=pow( (end_point_minus [kk] -ob[kk] [k] ) / (ob[kk+3 J [k] +6.0), ob[kk+6] [k] ) ; 

) 

potential_plus+=l . 0/cost_plus; 
potential_minus+=l . 0/cos t_jninus; 
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/ 


*************************** 
link midpoint collision 
*************************** 


************************* 
avoidance cost function 
******************** ***** 


★ 


/ 


cost _plus= -1.0; 
cost_jninus= -1,0; 
f or {kk=0 ; kk<3 ;kk++) 


cost_plus+=pow { 


(mid_point_plus [kk] -ob[kk] Ik] ) / (oblkk.3] [k] + 3 . 0) . ob[kk + 6] [k] ) j 


cost_niinus+=pow( (mid_point_minus [kk] -ob[kk] [k] ) / (ob[kk+31 [k) +3.0) ,ob[kk+6] [k] ) 

potent ial_plus+=l . 0/cost_plus; 
potent ial_minus+=l . 0 /cost_*tinus; 

} 


/***** ************************************ ********* 

* obstacle gradient vector 

********************************************** ****/ 

gradient [ j ] = (potential_plus-potential_*inus) / . 01 ; 
gradient_mag + =gradient [ j ] ‘gradient [ j ] ; 

/***★★******♦************************************** 

* rate only jacobian 

********** ***************♦************************/ 

for <i=0; i<3; i++) 

f or {k=0; k<4 ; k++) 

templi] [j3 [k] = ( result_plus t i ] [k] -result_mmus[i3 tk])/.01. 


} /* end virtual displacement loop */ 






/************♦** 

* normalize gradient vector ***********/ 


gradient_jnag= sqrt (gradient_mag) ; 
for (i=0; i<n w dof ; i++) 

gradient C i ] =gradient [ i ] /gradient^mag ; 


for (i=0;i<n_dof j i++) 

for ( j=0; j<n^dof ; j++) 

{ 

3 mu_jnatrix[i] tj ] = 1 • 0 -gradient [i] ‘gradient tj J ; 

else _ 

mu_matrix[i3 [j] --gradient [i] ‘gradient Id] ; 

} 


/*+* 

* 

* * * * 


obstacle avoidance jacobian 


* 4 ** 4 ****++/ 


for (k=0;k<4;k++) 


( 

for (i=0;i<3?i++) 
for ( j=0; j<n_dof ; j++) 


( 

jacobian[i] ( j] [k]=0.0; 
f or ( kk= 0 ; kk<n_do f ; kk+ + } 
jacobianfi] [ j] [k] +=mu_matrix [ j 3 


[kk] * t emp [ i 3 [kk] tk] ; 


> 


) 
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for <i=0; i<4; i++) 

{ 

free (result__plus [i ] ) ; 
free (resultjninus [i] ) ; 

) 

free (result_plus) ; 
free (result_minus) ; 

for ( i=0; i<3 ; i++ ) 

{ 

f or ( j = 0; j<i\_dof ; j + +) 
free (temp [i] ( j] ) ; 

free (temp [i] ) ; 

} 

free (temp) ; 
free (q_plus) ; 
free (q_jninus) ; 
free (gradient) ; 

return ( jacobi an) ; 

} 

/*******************4******* END effector position AND ERROR******************/ 

void encLef fector (int n, float*q_p, float target_p[3 ] [4) , f loat error_p[3] [4] , float err_mag[4]) 
{ 

int i * j ; 
float **result; 

result =IdentityMatrix ( 4) ; /*initialize transformation matrix*/ 

for (i=0 ; i<n; i++) /*carry out sequential matrix multiplication*/ 

mat rix_mult (result , transf ormation_matrix(i , g p) ) ; 

printf(’%.2f %.2f %.2f\n", result [0] [ 3 ] f result [1] [3] , result [2] [ 3] ) ; 

/**************************************** 

* determine end effector error 

♦ it**************************************/ 


for ( j=0; j<4; j++) 

{ 

for (i=0 ; i<3 ; i++) 

error_p[i) [ j ] =target_p [i ] [ j ] -result [i] [j] ; 

err_mag[ jj=sqrt (error_p[0] [ j] *error_p[0] [ j 3 +error_p[l] [ j J *error_p [1] [ j] +error_p [2] [j] *error_p[2] [ j] ) ; 
} 

for (i=0; i<4; i++) 

free (result [i] ) ; 
free (result) ; 

} 

/********************** * JOINT RATES** ** ***************************************/ 

float * joint _rates (int n, float *metric_p, float error_p [3] (4] , float target_p[3] [4] , 

float***jb, float step_s) 

{ 

float rate _jnag=0; 
float * rat e_p, error _jnag; 

int i,j,k; 

rate_p=mem_alloc_l (n) ; 


error_jnag=sqrt (error_p [0] [3] *error_p[0] [3 ] +error_p[l] [3] *error_pf 1] [3] +error_p [2] [3] *errorjp [2 J [3] ) ; 

/******** 4 ********************************** 4 ****** 

* target position approch rates 
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a************************®************************/ 

for ( i=0; i<n; i++) 

( 


rate_p[il =0; 

fOr<j=0 '«\ei7iU=metrlc_p[i]*jb[j] til [3J*error_p( jl (31 /errorjnag; 

rate_mag+=rate__p[i} *rate_p[i3 ; 


} 


/************ 


************************** 


* target 


orientation rates 
************************** 


************ 

***********/ 


for (i=0; i<n; i++) 

( 

for ( j=0; j<3; j++) 

for (k=0;k<3 ; k-M-) 

rate_p(i3 +=i0* jb[ j ] [x] [k] *target_p [ j 3 [k] ; 
rate_mag+=rate_p [ i 3 * rate_p [ i 3 » 

3 


rate_jnag=SQrt (rate_p&ag) ; 


* rate limit 10 degrees /sec 

**************************************** 




f or (i=0; i<n; i++) 


{ 

rate_p [ i 3 =rate_p ( i 3 /rate_mag ; 
if { flag { i] ==1 && fabs (rate_p [r 
rate_p [ i 3 = • 17 5 * st ep_s * rat e_jp [ i 3 


]) >.175*step_s) 

/sqrt (rate_p [i 3 *rate_p[ 


il) ; 


} 

return ( rat e_p) ; 


} 


/****♦**************** NUMERICAL INTEGRATION************** 
void integrate (int n, float *varl, float *var2, float err 

{ 

int i ; 
float step; 
float step_s; 


**/ 

float err_dot. 


float max^step) 


/* first order euler's method integration */ 
s t ep_s =max_s t ep ; 
if (err>10*step_s) 

{ 

if <err>fabs(err_dot) ) 
step=step_s; 

else 

step=fabs(err/err_dot) *step_s; 

) 

else 

step= . 25* step_s ; 


printf ( * % * 2 f \n*,step); 

f or (i=0; i<n; i++) 

var2 [i ] +=varl t i 1 *step; 

> 
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/*********************** print OUTPUT* *********************************/ 
void joints_print (int n, float *var,FILE *file) 

{ 

int i ; 

for ( i=0; i<n/2 ; i++) 

fprintf (file, "%f ", var[i]); 

fprintf (file, "\n"); 

for (i=n/2 ; i<n; i4- + ) 

fprintf (file, "%f ", var[i]); 

fprintf (file, "\n“); 

} 

/A************************* MEMORY ALLOCATION !******•*********»**************** *♦**/ 

float *mem_alloc_l (int n) 

( 

float *var; 

if((var = (float *) malloc (si 2 eof ( float) *n) ) == (float *) NULL) 

{ 

fprint f (stderr, "mallocing error\n“); 
exit (-1 ) ; 

) 

return (var) ; 

> 

/*************** *********** MEMO ry ALLOCATION 2*************************************/ 

float **mem_alloc_2 (int nrows,int ncols) 

{ 

float **var; 
int i; 

if ( (var= ( float **)malloc (sizeof (float*) *nrows) ) *= (float **) NULL) 

{ 

fprintf (stderr, " mallocing error\n“); 
exit (-1) ; 

) 

for (i=0; i<nrows; i++) 

{ 

if ( (var[i] = (float* ) malloc (sizeof (float) *ncols) ) == (float *) NULL) 

{ 

fprintf (stderr, " mallocing error\n*); 
exit (-1) ; 

) 

) 

return (var) ; 

) 

/************************ ******OBSTACLE ORIENTATION TRANSFORMATION*********/ 
void **obstacle_transformation(int n, float **obst, float vector_n[3]) 

{ 

float pry [3] [3]; 
float vector_r[3]; 
int i,j; 

pry [0] [0] =cos (obst (11] [n] ) *cos (obst [10] [n] ) ; 

pry [0] [1] =cos (obst [11 ) [n] ) * sin (obst [10] [n] ) * sin (obst [9] [n] ) -sin (obst [11] [n] ) *cos(obst [9] [n] ) ; 
pry [0] [2] =cos (obst [11] [n] ) * sin (obst [ 10] [n] ) *cos(obst [9] [n] ) +sin(obst [11] [n] ) * sin (obst [9] [n] ) ; 

pry [1] [0]=sin(obst [11] [n] ) *cos (obst [ 10] [n] ) ; 

pry [1] (l]=sin(obst [11] [n] ) * sin (obst [ 10] [n] ) * sin (obst [9] [n] ) +cos (obst [11] [n] ) *cos(obst [9] [n] ) ; 
pry [ 1] [2]=sin(obst [11] [n] ) *sin(obst [10] [n] ) *cos(obst[9] [n] } -cos (obst [11] [n] ) *sin(obst[9] [n] ) ; 
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pry [2] [0]=-sin(obst [10] [n]) ; 

pry 12] [1] =cos (obst [10] [n] ) *sin(obst [9] [n] } 

pry [2] [2]=cos(obst [10] [n] ) *cos (obst [ 9] [n]) 


for (i=0; i<3; i++) 

{ 

vector_r [ i ] =0; 

for { j=0; j<3; j++) 

vector_r [ i 3 +=pry [i ] [ j 3 *vector_n [ j ] 
} 

f or (i=0 ; i<3; i++) 

vector_n l i 3 =vector_r [ i ] ; 

} 
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